#include "../_includes.h"

bool s_move_force_stop_command_set = false;
void arc_move(int dir, double r, double theta_deg_abs,
              double max_outter_wheel_speed, int timeout) {
  double r_abs = abs(r);
  const double wheel_width = 260; // mm
  double v_out = max_outter_wheel_speed;
  double v_in = v_out / r_abs * (r_abs - wheel_width);
  if (r > 0)
    chassis.LR_set_speed(dir * v_out, dir * v_in);
  else
    chassis.LR_set_speed(dir * v_in, dir * v_out);
  // double arc_length_center = theta_deg / 180 * PI * r_abs;
  timer t;
  while (!s_move_force_stop_command_set && t.time() < timeout) {
    // if (chassis_dst_mm_avg() > arc_length_center) {
    //   cout << "dist break\n";
    //   break;
    // }
    if (abs(get_gyro()) > abs(theta_deg_abs))
      break;
    wait(1);
  }
  chassis.stop();
}

bool s_move_running = false;

//！！！！！本函数会清零陀螺仪
//不准,勉强使用
//供s_move_with_gryo_reset_routine使用的全局变量
static double dx_0, dy_0, max_outter_speed_0, timeout_0 = 0;
void s_move_with_gryo_reset_routine() {
  s_move_running = true;
  dx_0 *= 0.95; // x方向有偏移
  dy_0 *= 0.95; // Y方向刹车不准

  double hdx = dx_0 / 2;
  double hdy = dy_0 / 2;
  double r = (hdx * hdx + hdy * hdy) / 2 / hdx;
  double theta_deg = asin(hdy / r) / PI * 180; // deg
  cout<< theta_deg<<endl;

  gyro_reset();
  // chassis.reset_enc();
  arc_move(sgn(dy_0), r, abs(theta_deg), max_outter_speed_0, timeout_0 / 2);
  // chassis.reset_enc();
  gyro_reset();
  arc_move(sgn(dy_0), -r, abs(theta_deg), max_outter_speed_0, timeout_0 / 2);
  s_move_running = false;
}

void s_move_with_gryo_reset(double dx, double dy, double max_outter_speed,
                            int timeout, Block waitForCompletion) {
  dx_0 = dx;
  dy_0 = dy;
  max_outter_speed_0 = max_outter_speed;
  timeout_0 = timeout;

  s_move_force_stop_command_set = false;
  thread t(s_move_with_gryo_reset_routine);
  //等待完成
  if ((bool)waitForCompletion & !s_move_force_stop_command_set)
    while_wait(t.joinable());
  t.interrupt();
}

void s_move_force_stop() { s_move_force_stop_command_set = true; }